﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Utils.Physics2D.Objects;
using Utils.MathUtilitys;

namespace Utils.Physics2D.Collision
{
    public class RectCollider : CollisionHandler
    {
        public const int sides = 4;

        Vector2[] corners = new Vector2[sides];
        Vector2[] edges = new Vector2[sides];
        Vector2[] normals = new Vector2[sides];
        float width;
        float height;

        public RectCollider(PhysicsObject owner, Vector2 a, Vector2 b, Vector2 c, Vector2 d) : base(owner)
        {
            UpdatePositions(a, b, c, d);
            //TODO: Assertion if Rect is not orthogonal?
        }

        public void UpdatePositions(Vector2 a, Vector2 b, Vector2 c, Vector2 d)
        {
            corners[0] = a;
            corners[1] = b;
            corners[2] = c;
            corners[3] = d;

            for(int i=0; i<sides; i++)
            {
                edges[i] = corners[Next(i)] - corners[i];
                normals[i] = new Vector2(-edges[i].Y, edges[i].X);
                normals[i].Normalize();
            }
            width = edges[0].Length();
            height = edges[1].Length();
        }

        int Next(int side)
        {
            return (side + 1) % sides;
        }

        int Prev(int side)
        {
            return side == 0 ? sides-1 : side-1;
        }

        public override bool CheckCollision(PhysicsObject obj, DISPLACE displacement)
        {
            Vector2 p = obj.Pos;
            float r = obj.Radius;
            Vector2 ap = p - corners[0];
            Vector2 ad = - edges[3];
            float dotApAb = Vector2.Dot(ap, edges[0]);
            float dotAbAb = Vector2.Dot(edges[0], edges[0]);
            float dotApAd = Vector2.Dot(ap, ad);
            float dotAdAd = Vector2.Dot(ad, ad);

            // DOTO: optimize for non displacement
            bool inside = 0 <= dotApAb && dotApAb <= dotAbAb && 0 <= dotApAd && dotApAd <= dotAdAd; // checks if point is inside rect
            bool[] colEdge = new bool[sides]; // check if circle intersects with one of the edges
            for (int i = 0; i < sides; i++)
            {
                colEdge[i] = CollisionUtils.CircleLine(corners[i], corners[Next(i)], p, r);
            }

            if(inside || colEdge[0] || colEdge[1] || colEdge[2] || colEdge[3])
            {
                if (displacement != DISPLACE.NONE)
                {
                    // find the direction the object have to be pushed in
                    Vector2 impactDir = Vector2.Zero;

                    if (!inside)
                    {
                        // if outside of the rect, push it to the side the point is on
                        Vector2 center = corners[0] + (corners[2] - corners[0]) * 0.5f;
                        Vector2 offset = Vector2.Zero;
                        if (width > height)
                        {
                            offset = edges[0] * ((width - height) / width * 0.5f * MathUtils.Sign(Vector2.Dot(p - center, edges[0])));
                        }
                        else if (height > width)
                        {
                            offset = edges[1] * ((height - width) / height * 0.5f * MathUtils.Sign(Vector2.Dot(p - center, edges[1])));
                        }
                      
                        impactDir = center + offset - p;
                    }
                    if(impactDir == Vector2.Zero)
                    {
                        // else, push it to the side it came in
                        Vector2 objDir = obj.Pos - obj.OldPos;
                        Vector2 selfDir = owner.Pos - owner.OldPos;
                        impactDir = objDir - selfDir;
                    }

                    if (impactDir != Vector2.Zero)
                    {
                        float[] collinearitys = new float[sides];
                        int collinearityMost = 0;

                        for (int i = 0; i < sides; i++)
                        {
                            collinearitys[i] = Vector2.Dot(-impactDir, normals[i]);
                            if (collinearitys[i] > collinearitys[collinearityMost])
                            {
                                collinearityMost = i;
                            }
                        }

                        Vector2 pushDir;

                        float colPrevCorner = Vector2.Dot(p - corners[collinearityMost], -edges[collinearityMost]);
                        float colNextCorner = Vector2.Dot(p - corners[Next(collinearityMost)], edges[collinearityMost]);
                        if (
                            !inside
                            && colEdge[collinearityMost] 
                            && ((colEdge[Prev(collinearityMost)] && colPrevCorner > 0) 
                                || (colEdge[Next(collinearityMost)] && colNextCorner > 0)))
                        {
                            // push away from an edge
                            Vector2 cornerPoint;
                            if(colEdge[Prev(collinearityMost)])
                            {
                                cornerPoint = corners[collinearityMost];
                            }
                            else {
                                cornerPoint = corners[Next(collinearityMost)];
                            }

                            pushDir = p - cornerPoint;
                            float length = obj.Radius - pushDir.Length();
                            pushDir.Normalize();
                            pushDir *= length;
                        }
                        else
                        {
                            // push away into normal direction
                            float t = Vector2.Dot(p - corners[collinearityMost], edges[collinearityMost]) / edges[collinearityMost].LengthSquared();
                            float dis = r - (corners[collinearityMost] + edges[collinearityMost] * t - p).Length();
                            pushDir = normals[collinearityMost] * dis;
                        }

                        PushByDir(obj, displacement, pushDir);
                    }
                }

                return true;
            }
            return false;
        }
    }
}
